/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import java.util.Date;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.*;
import edu.wpi.first.wpilibj.SensorBase.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class KarthikJTest extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
     Joystick joyRight = new Joystick (1);                                      // Right Joystick is in Port 1
     Joystick joyLeft = new Joystick (2);                                       // Left Joystick is in Port 2
     RobotDrive Tank = new RobotDrive (1,2,3,4);                                // Their are 4 motors
     Gyro Orange = new Gyro(2);                                                 // Gyro is on Port 2; It's name is Orange
     String GyroAngle = String.valueOf(GetGyroAngle());                         // Converts the values of the Gyro into a string
     int GyroCount = 1;                                                         // The first variable for the Gyro Counter
     AxisCamera camera;
     ColorImage image;
     Relay CameraLight = new Relay(1);
     // long StartTime = 0;

    public void robotInit()
    {

       Orange.reset();                                                          // Reset's the Gyro position
       camera = AxisCamera.getInstance();
       camera.writeBrightness(0);
       camera.writeCompression(0);
       camera.writeResolution(ResolutionT.k320x240);

    }
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic()
    {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopContinuous()
    {
        /* long Now = new Date().getTime();                                        // Code was used to test how frequency of Teleop Loop
        System.out.println("Frequency: " + (1000.0f/(Now-StartTime)));
        StartTime = Now; */
        Tank.tankDrive(joyRight, joyLeft);
        CameraImage();
        updateDashboard();
        DisplayGyroAngle();

    }

    public void DisplayGyroAngle()                                              // Method for Displaying Gyro Angle on Driver Station
    {
        System.out.println("The Angle of the gyro is approximately:" + GetGyroAngle());
        Timer.delay(1);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, GyroCount + " Gyro Angle = " + GetGyroAngle() );
        DriverStationLCD.getInstance().updateLCD();
        GyroCount += 1;

    }
    public double GetGyroAngle()                                                // Method for finding the Gyro values between -180 to 180
    {
        double original = Orange.getAngle();
        while(original < -180)
            original = (original += 360);
        while (original > 180)
            original = (original -= 360);
        return original;
    }
    public void CameraImage()                                                   // Method for getting Camera Image and turning on the Camera Lights
    {
        CameraLight.setDirection(Relay.Direction.kForward);
        CameraLight.set(Relay.Value.kOn);
     if (camera.freshImage() == true){
            try {
                image = camera.getImage();
                DriverStationLCD.getInstance().updateLCD();
                image.free();

                }
            catch (AxisCameraException ex) {
                    ex.printStackTrace();
                }
            catch (NIVisionException ex) {
                    ex.printStackTrace();
                }
        }
    }

    void updateDashboard() {
                 Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            // lowDashData.addByte(Solenoid.getAll());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

     }
}
